#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/segmentation/sac_segmentation.h>
#include<pcl/sample_consensus/method_types.h> //模型定义头文件
#include<pcl/sample_consensus/model_types.h> //随机参数估计方法头文件
#include <pcl/filters/extract_indices.h>
#include <pcl/point_types.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>




using namespace std;

int main()
{

    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);			//待滤波点云
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>);	//滤波后点云
    cout << "->正在读入点云..." << endl;
    pcl::PCDReader reader;
    reader.read("out.pcd", *cloud);
    cout << "\t\t<读入点云信息>\n" << *cloud << endl;


    ///读入点云数据

    ///体素滤波器点云下采样
    cout << "->正在体素下采样..." << endl;
    pcl::VoxelGrid<pcl::PointXYZ> vg;		//创建滤波器对象
    vg.setInputCloud(cloud);				//设置待滤波点云
    vg.setLeafSize(0.05f, 0.05f, 0.05f);	//设置体素大小
    vg.filter(*cloud_filtered);			//执行滤波，保存滤波结果于cloud_filtered

    ///保存下采样点云
    cout << "->正在保存下采样点云..." << endl;
    pcl::PCDWriter writer;
    //writer.write("sub.pcd", *cloud_filtered, true);
    cout << "\t\t<保存点云信息>\n" << *cloud_filtered << endl;


    cout << "->正在进行统计滤波..." << endl;
    pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;	//创建滤波器对象
    sor.setInputCloud(cloud);							//设置待滤波点云
    sor.setMeanK(50);									//设置查询点近邻点的个数
    sor.setStddevMulThresh(0.05);						//设置标准差乘数，来计算是否为离群点的阈值
    //sor.setNegative(true);							//默认false，保存内点；true，保存滤掉的离群点
    sor.filter(*cloud_filtered);						//执行滤波，保存滤波结果于cloud_filtered

    ///保存下采样点云
    cout << "->正在保存滤波点云..." << endl;
    //pcl::PCDWriter writer;
    //writer.write("StatisticalOutlierRemoval.pcd", *cloud_filtered, true);
    cout << "\t\t<保存点云信息>\n" << *cloud_filtered << endl;
    std::vector<int> mapping;
    pcl::removeNaNFromPointCloud(*cloud, *cloud, mapping);

    cout << "->正在进行半径滤波..." << endl;
    pcl::RadiusOutlierRemoval<pcl::PointXYZ> ror;	//创建滤波器对象
    ror.setInputCloud(cloud);						//设置待滤波点云
    ror.setRadiusSearch(0.02);						//设置查询点的半径范围
    ror.setMinNeighborsInRadius(5);					//设置判断是否为离群点的阈值，即半径内至少包括的点数
    //ror.setNegative(true);						//默认false，保存内点；true，保存滤掉的外点
    ror.filter(*cloud_filtered);					//执行滤波，保存滤波结果于cloud_filtered

    ///保存下采样点云
    cout << "->正在保存滤波点云..." << endl;
    //pcl::PCDWriter writer;
    //writer.write("filtered.pcd", *cloud_filtered, true);
    cout << "\t\t<保存点云信息>\n" << *cloud_filtered << endl;

    pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
    pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
    pcl::SACSegmentation<pcl::PointXYZ> seg;
    seg.setOptimizeCoefficients(true);
    seg.setModelType(pcl::SACMODEL_PLANE);
    seg.setMethodType(pcl::SAC_RANSAC);
    seg.setDistanceThreshold(0.01);
    seg.setInputCloud(cloud);
    seg.segment(*inliers, *coefficients);
    pcl::ExtractIndices<pcl::PointXYZ> my_extract_indices;
    my_extract_indices.setInputCloud(cloud);
    my_extract_indices.setIndices(inliers);
    my_extract_indices.setNegative(false);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p (new pcl::PointCloud<pcl::PointXYZ>);
    my_extract_indices.filter(*cloud_p);


    pcl::PCDWriter writer1;
    writer1.write("ground.pcd", *cloud_p, true);

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
    tree->setInputCloud(cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
    ec.setClusterTolerance(0.02);
    ec.setMinClusterSize(100);
    ec.setMaxClusterSize(25000);
    ec.setSearchMethod(tree);
    ec.setInputCloud(cloud_filtered);
    ec.extract(cluster_indices);

    //迭代访问点云索引cluster_indices，直到分割出所有聚类


    int j = 0;
    //遍历整个索引集合cluster_indices
    for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>); //创建空的点云集
        //遍历每个索引集合中的每个点，将每个点存入创建的空的点云集合中
        for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++) //为空的点云集中逐个赋对象
        {
            cloud_cluster->points.push_back(cloud_filtered->points[*pit]);
        }
        cloud_cluster->width = cloud_cluster->points.size();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;
        cout<<"该聚类点云中点云的点的数量为 : "<< cloud_cluster->points.size()<<endl;
        std::stringstream ss;
        ss << "cloud_cluster_" << j << ".pcd";
        writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false);
        j++;
    }









//================================= 滤波前后对比可视化 ================================= ↓

    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("vs"));

    /*-----视口1-----*/
    int v1(0);
    viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); //设置第一个视口在X轴、Y轴的最小值、最大值，取值在0-1之间
    viewer->setBackgroundColor(0, 0, 0, v1); //设置背景颜色，0-255，默认黑色（0，0，0）
    viewer->addText("befor_filtered", 10, 10, "v1_text", v1);
    viewer->addPointCloud<pcl::PointXYZ>(cloud, "befor_filtered_cloud", v1);

    /*-----视口2-----*/
    int v2(0);
    viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);
    viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);
    viewer->addText("after_filtered", 10, 10, "v2_text", v2);
    viewer->addPointCloud<pcl::PointXYZ>(cloud_filtered, "after_filtered_cloud", v2);

    /*-----设置相关属性-----*/
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "befor_filtered_cloud", v1);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1, 0, 0, "befor_filtered_cloud", v1);

    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "after_filtered_cloud", v2);
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 1, 0, "after_filtered_cloud", v2);

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        //boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }

    //================================= 滤波前后对比可视化 ================================= ↑

    return 0;
}
